/*!  \file  GK.cpp
 *	  \brief  
 * 
 *   \details   details
 *   \author    Yordan Madzhunkov German Research School for Simulation Sciences GmbH
 *   \version   1.0
 *   \date      09/02/2013 01:22:30 PM
 *   \copyright GNU Public License.
 */
#include <iostream>
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <float.h>
#include <iomanip>
#include "CompileTime.h"
#include "Matrix.h"
#include "GK.h"
using namespace std;
#pragma GCC diagnostic ignored "-Wunused-parameter"

int GK::spaceNeeded(const int n, const Representation rep) {
	return  ((rep != NGN) ? 4 * n : 10 * n);

}
void GK::copyFields(const GK & mat) {
	nat      = mat.nat;
	sig      = mat.sig;
	pivotMin = mat.pivotMin; // holds the pivot min threshold
	maxNat   = mat.maxNat; // abs maximum offdiagonal element
	spdiam   = mat.spdiam; // estimate of diameter of the matrix
	n        = mat.n;
}
const char * GK::getRepresentationName() const {
	static const char * names[] = { "Native", "LDL", "URU", "NGN" };
	return names[representation];
}


void GK::construct(const Representation rep, const int n_, double * RS allocateData, const int space_){
	n = n_;	representation = rep;
	setPointers();
	setPivotMin(0);
	repIndex = -1;
	nat = NULL;
	sig = NULL;
	sigAllocated = false;
	if (space_ >= spaceNeeded(n, rep) && allocateData != NULL ) {
		space = space_;
		data = allocateData;
		selfAllocated = false;
	} else {	MISSING;	}
}

GK::GK(const Representation rep, const int n_, double * RS allocateData, const int space_) {
	construct(rep, n_, allocateData, space_);
	nat = NULL;
	sig = NULL;
	sigAllocated = false;
}


GK::GK(const Representation rep, const int n_) {
	n = n_;	representation = rep;
	setPointers();
	space = spaceNeeded(n, rep);
	data = new double[space];
	selfAllocated = true;
	sig = NULL;
	sigAllocated = false;
	setPivotMin(0);
	repIndex = -1;
}

GK::GK(const Matrix & B, const double shift_){ // create from bidiagonal matrix

	n = B.rows();
	representation = Native;
	setPointers();

	space = spaceNeeded(n, representation);
	data = new double[space];
	selfAllocated = true;

	int * sig_mod = new int[spaceSignature(n)];
	sigAllocated = true;

	setPivotMin(0);
	repIndex = -1;

	biDiagonalToNative(B.getData(),  B.getOffDiagonal(), sig_mod, shift_);

	sig = sig_mod;
	nat = data;

}
void GK::biDiagonalToNative(const double * RS d, const double * RS e, 
									int * RS signature,  const double shift) {

	double * RS d2 = data;	
	*d2 = 0;//shift;
	d2++;	
	double max = 0;
	bool cond = false;
	spdiam = 0; max = 0;
	const int nBits = sizeof(int)  * 4;
	const unsigned int limitI = (n-1) / nBits;
	for (unsigned int i = 0 ; i < limitI; i++) {
		unsigned int s = 0;
		for (unsigned int k = i * nBits; k < (i+1) * nBits; k++) {
			s = (s << 1) + cond;
			int j = k << 1;
			bool neg = d[k] < 0;
			cond = neg xor cond;
			d2[j] = neg ? -d[k] : d[k]; 

			max    = fmax(max, d2[j]);
			spdiam = fmax(spdiam, d2[j] + d2[j-1]);
			
			s = (s << 1) + cond;
			neg = e[k] < 0;
			cond = neg xor cond;
			d2[j + 1] = neg ? -e[k] : e[k]; 

			max    = fmax(max, d2[j+1]);
			spdiam = fmax(spdiam, d2[j+1] + d2[j]);
		}
		signature[i] = s;
	}

	int i = limitI;
	unsigned int s = 0;
	for (int k = i * nBits; k < n - 1; k++) {
		s = (s << 1) + cond;
		int j = k << 1;
		bool neg = d[k] < 0;
		cond = neg xor cond;
		d2[j] = neg ? -d[k] : d[k]; 

		max    = fmax(max, d2[j]);
		spdiam = fmax(spdiam, d2[j] + d2[j-1]);

		s = (s << 1) + cond;
		neg = e[k] < 0;
		cond = neg xor cond;
		d2[j +1] = neg ? -e[k] : e[k]; 

		max    = fmax(max, d2[j+1]);
		spdiam = fmax(spdiam, d2[j+1] + d2[j]);
	}

	int k = n - 1;
	int j = k << 1;
	s = (s << 1) + cond;
	bool neg = d[k] < 0;
	cond = neg xor cond;
	d2[j] = neg ? -d[k] : d[k]; 
	s = (s << 1) + cond;
	max    = fmax(max, d2[j]);
	spdiam = fmax(spdiam, d2[j] + d2[j-1]);

	maxNat = max;
	setPivotMin(DBL_EPSILON * max * max);

	signature[i] = s;
	*data = shift;
		

}
void GK::biDiagonalToNative(const double * RS d, const double * RS e, 
	const double shift) {
	assert(d != NULL && e != NULL);
	double * RS d2 = data;
	*d2 = shift; d2++;
	double max = 0;
	double t1 = 0;
	double t2 = fabs(d[0]);
	*d2 = d[0];	max = fmax(max, t2); d2++;
	for (int i = 0; i < n-1; i++ ) {
		t1 = t2; t2 = fabs(e[i]); 
		spdiam = fmax(spdiam, t1 + t2);
		*d2 = e[i  ];	max = fmax(max, t2); d2++;
		t1 = t2; t2 = fabs(d[i+1]); 
		spdiam = fmax(spdiam, t1 + t2);
		*d2 = d[i+1];	max = fmax(max, t2); d2++;
	}
	maxNat = max;
	setPivotMin(DBL_EPSILON * max * max);

}
GK::GK( const Matrix & B, double * RS allocateData, const int space_,
	const double shift_ ){ // create from bidiagonal matrix
	int N = B.rows();
	construct(Native, N, allocateData, space_);
	biDiagonalToNative(B.getData(),  B.getOffDiagonal(), shift_);
	nat = data;
	sig = NULL;
	sigAllocated = false;
	repIndex = -1;
}
void GK::delocate() {
	if (selfAllocated) {	if (data) delete[] data;	}
	if (sigAllocated ) { if (sig) delete[] sig; };
	data = 0; n = 0; nat = 0; sig = 0;
}

GK::GK(const Representation rep, const GK & oth) {
	copyFields(oth);
	representation = rep; 
	setPointers();
	repIndex = -1;

	space = spaceNeeded(n, rep);
	data = new double[space]; 
	selfAllocated = true;
	sigAllocated = false;

}
GK::GK(const Representation rep, const GK & oth, const double relShift) {
	copyFields(oth);
	representation = rep;
	setPointers();
	repIndex = -1;

	space = spaceNeeded(n, rep);
	data = new double[space];
//	memset(data, 0, sizeof(data[0]) * space);
	selfAllocated = true;
	sigAllocated = false;

	shiftFrom(oth, relShift);
}

GK::GK(const Representation rep, const GK & oth, double * RS allocateData, 
			const int space_,  const double relShift) {

	int N = oth.n;	representation = rep;
	construct(rep, N, allocateData, space_);
	sigAllocated = false;
	copyFields(oth);
	setPointers();
	repIndex = -1;
	shiftFrom(oth, relShift);
}


void GK::initializeLDL   (const double shift){
	// LDL memory storage pattern
	// data [0]  data[1] data[2] data[3] data[4] .. data[2n -2] data[2n -1]
	// shift			 d[0]    l[0]    d[1]    l[1] ..    l[n-2]      d[n-1]
	// T = LDL' - shift * I
	data[0] = shift;
	data[1] = pivot(shift);                     // D[0] 
	for (int i = 1; i < 2 * n ; i++) {
		data[2 * i + 0 ] = nat[i    ]      / data[2 * i - 1]; // L[2 * i - 1]
		data[2 * i + 1 ] = pivot(shift-nat[i]* data[2 * i + 0]);// D[2 * 1    ]
	}
};
void GK::initializeURU   (const double shift){
	data[4 * n  - 1 ] = pivot(shift); 								    // R[N - 1]
	data[4 * n  - 2 ] = nat[2*n-1] /  data[4 * n  - 1 ];			    // U[N - 1]
	data[4 * n  - 3 ] = pivot(shift - nat[2*n-1] * data[4 * n  - 2]);// R[N - 2]
	for (int i = n - 1; i > 0; i--) {
		data[4 * i + 0 ] = nat[2*i]        / data[4 * i + 1]; 
		data[4 * i - 1 ] = pivot(shift  - nat[2*i  ]* data[4 * i + 0]); 
		data[4 * i - 2 ] = nat[2*i-1]      / data[4 * i - 1]; 
		data[4 * i - 3 ] = pivot(shift  - nat[2*i-1] * data[4 * i - 2]); 
	}
	data[0] = shift;
};
// computes  NGN from native rep + shift
void GK::initializeNGN   (const double shift){
	double * RS uru   = data + 4 * n;
	double * RS gamma = uru  + 4 * n;
	assert(nat != NULL);
	repIndex = -1;

	// perform LDL
	data[0]  = shift;
	gamma[0] = 0; 						// gamma[0]
	data[1]  = pivot(shift);      // D[0] 
	for (int i = 1; i < 2*n ; i++) {
		data[2 * i + 0 ] =   nat[i] / data[2 * i - 1];          // L[2 * i - 1]
		gamma[i        ] = - nat[i] * data[2 * i + 0];        // G[2 * i    ]
		data[2 * i + 1 ] = pivot(shift  + gamma[i]);      // D[2 * 1    ]
	}

	// perform URU
	uru  [4 * n - 1]  = pivot(shift);        					   // R[N - 1]
	gamma[2 * n - 1] += uru[4 * n  - 1 ];

	double minGamma = fabs(gamma[2 * n - 1]), tmp = 0; int k = 2 * n - 1;
	for (int i = 2 * n - 1; i > 0; i--) {
	  	uru[2 * i + 0 ] = nat[i]    / uru[2 * i + 1]; 
		uru[2 * i - 1 ] = pivot(shift - nat[i] * uru[2 * i + 0]);
		gamma[i - 1] += uru[2 * i  - 1 ];

		tmp = fabs(gamma[ i - 1]);
		k = tmp < minGamma ? i - 1 : k;
		minGamma = fmin(tmp, minGamma);

	}
	uru[0] = shift;  
	minGammaIndex = k;

};
void GK::initializeNGNPack   (const double shift){
	MISSING;
}
void GK::addShiftNative(double const& s) {
	*data += s;
}
void GK::addShiftLDL   (double const& relShift) {
	double s = 0;
	double t = relShift; // assume this will be the change in shift
	data[0] += t;
	// using N representation
	for (int i = 0; i < 2 * n - 1 ; i++) {
 	 	data[2 * i + 1 ] = pivot(data[2 * i + 1] + s + t);
		s = (s + t) * data[2 * i + 2 ];
		data[2 * i + 2 ] = nat[i + 1]  / data[2 * i + 1]; 
		s *= data[2 * i + 2];
	}
	data[4 * n - 1] = pivot(data[4 * n - 1] + s + t);
}
void GK::addShiftURU   (double const & relShift) {
	double s = 0;
	double t = relShift; 
	for (int i = 2 * n - 1; i > 0; i--) {
 	 	data[2 * i + 1 ] = pivot(data[2 * i + 1] + s + t);
		s = (s + t) *  data[2 * i    ];
		data[2 * i     ] = nat[i    ]  / data[2 * i + 1]; 
		s *= data[2 * i    ];
	}
	data[1] = pivot(data[1] + s + t);
	data[0] = data[0] + t;
}
void GK::addShiftNGN   (double const& s) {
	double newShift = getShift() + s;
	initializeNGN(newShift);
}
void GK::addShiftNGNPack   (double const& relShift) {
	const int k = getRepIndex();
	double s = 0;
	double t = relShift; // assume this will be the change in shift
	data[0] += t;
	for (int i = 0; i < k ; i++) {
 	 	data[2 * i + 1 ] = pivot(data[2 * i + 1] + s + t);
		s = (s + t) * data[2 * i + 2 ];
		data[2 * i + 2 ] = nat[i + 1]  / data[2 * i + 1]; // L[2 * i - 1]
		s *= data[2 * i + 2];
	}
	data[2 * k + 1] += s + t; // gamma_k
	s = 0;
	for (int i = 2 * n - 1; i > k; i--) {
 	 	data[2 * i + 1 ] = pivot(data[2 * i + 1] + s + t);
		s = (s + t) * data[2 * i    ];
		data[2 * i     ] = nat[i    ]  / data[2 * i + 1];
		s *= data[2 * i    ];
	}
	data[2 * k + 1] += s;
}

void GK::initializeNative(const double shift){
	*data = shift;
};

void GK::nativeToLDL   (const GK & mat, const double shift_){
	assert(this->representation == LDL    && mat.getRepresentation() == Native);
	const double * RS root = mat.getData();
	const double shift = root[0] + shift_;
	nat = mat.nat;

	data[0] = shift ;
	data[1] = pivot(data[0]); 
	for (int i = 1; i < 2 * n ; i++) {
		data[2 * i + 0 ] = root[i]      / data[2 * i - 1];
		data[2 * i + 1 ] = pivot(shift- root[i] * data[2 * i + 0]);
	}

};
void GK::nativeToURU   (const GK & mat, const double shift_){
	assert(this->representation == URU    && mat.getRepresentation() == Native);
	const double * RS root = mat.getData();
	const double shift = root[0] + shift_;

	nat = mat.nat;

	data[4 * n  - 1 ] = pivot(shift); 								    // R[N - 1]
	for (int i = 2 * n - 1; i > 0; i --) {
		data[2 * i     ] = root[i] / data[2 * i + 1];
		data[2 * i - 1 ] = pivot(shift- root[i] * data[2 * i]);
	}
	data[0] = shift;

};
void GK::nativeToNGN   (const GK & mat, const double shift_){
	assert(this->representation == NGN  && mat.getRepresentation() == Native);
	const double * RS root = mat.getData();
	const double shift = root[0] + shift_;
	double * RS ldl   = data + 0 * n;
	double * RS uru   = data + 4 * n;
	double * RS gamma = uru  + 4 * n;
	repIndex = -1;

	nat = mat.nat; 

	ldl[0] = shift ;
	ldl[1] = pivot(ldl[0]); 
	gamma[0] = 0;
	for (int i = 1; i < 2 * n ; i++) {
		ldl[2 * i + 0 ] =  root[i] / ldl[2 * i - 1];
		gamma[i]        = -root[i] * ldl[2 * i + 0 ]; 
		ldl[2 * i + 1 ] = pivot(shift + gamma[i]);
	}
	minGammaIndex = 2 * n - 1;
	
	uru[4 * n  - 1 ] = pivot(shift); 								    // R[N - 1]
	gamma[2 * n - 1] += shift;
	double val = fabs( gamma[2 * n - 1]);
	for (int i = 2 * n - 1; i > 0; i --) {
		uru[2 * i     ] = root[i] / uru[2 * i + 1];
		uru[2 * i - 1 ] = pivot(shift- root[i] * uru[2 * i]);
		gamma[i - 1] += uru[2 * i  - 1 ]; 
		updateMinGamma(gamma[i-1], i-1, val);
	}
	uru[0] = shift;
};
void GK::nativeToNGNPack   (const GK & mat, const double shift_){
	assert(this->representation == NGNPack  && mat.getRepresentation() == Native);
	MISSING;
}
// this is kind of pointless, but I implemented for consistancy
void GK::nativeToNative(const GK & mat, const double shift_){
	assert(this->representation == Native && mat.getRepresentation() == Native);
	const double * RS root = mat.getData();
	data[0] = root[0] + shift_;
	nat = mat.nat;
	for (int i = 1; i < 2 * n; i++)
		data[i] = root[i];

};

// LDL -> LDL is also known as dstqds
void GK::LDLToLDL      (const GK & mat, const double relShift){
	assert(representation == LDL  && mat.getRepresentation() == LDL);
	assert(n == mat.getN() && nat == mat.getNat());
	assert(this != &mat);
	double s = 0;
	double t = relShift; // assume this will be the change in shift
	const double * RS root = mat.getData();
	data[0] = root[0] + t;
	// using N representation
	for (int i = 0; i < 2 * n - 1 ; i++) {
 	 	data[2 * i + 1 ] = pivot(root[2 * i + 1] + s + t);
		data[2 * i + 2 ] = nat[i + 1]  / data[2 * i + 1]; // L[2 * i - 1]
		s = (s + t) * data[2 * i + 2 ] * root[2 * i + 2];
	}
	data[4 * (n-1) + 3 ] = pivot(root[4 * (n-1) + 3] + s + t);
};
//Flipped dqds
void GK::LDLToURU      (const GK & mat, const double relShift){	
	assert(this->representation == URU    && mat.getRepresentation() == LDL);
	assert(n == mat.getN() && nat == mat.getNat());
	const double t = relShift;
	const double * RS ldl = mat.getData();	
	double delta = 0 + ldl[4 * n - 1] + t;
	for (int i = 2 * n - 1; i > 0; i--) {
		data[2 * i + 1] = pivot(nat[i] * ldl[2 * i] + delta);
		data[2 * i    ] = nat[i] / data[2 * i + 1];
		delta *= ldl[2 * i - 1] / data[2 * i + 1];
		delta += t;
	}
	data[1] = delta;
	data[0] = mat.getShift() + t;
};
void GK::LDLToNGN      (const GK & mat, const double relShift){
	assert(this->representation == NGN    && mat.getRepresentation() == LDL);
	assert(n == mat.getN() && nat == mat.getNat());
	repIndex = -1;
	double s = 0;
	double t = relShift; // assume this will be the change in shift
	const double * RS ldl   = mat.getData();
	double * RS uru   = data + 4 * n;;	
	double * RS gamma = uru  + 4 * n;;	
	data[0] = ldl[0] + t;
	// using N representation
	for (int i = 0; i < 2 * n  ; i++) {
		gamma[i] = s;
 	 	data[2 * i + 1 ] = pivot(ldl[2 * i + 1] + s + t);
		data[2 * i + 2 ] = nat[i + 1]  / data[2 * i + 1]; // L[2 * i - 1]
		s = (s + t) * data[2 * i + 2 ] * ldl[2 * i + 2];
	}
//	cout << "d[" << 2 * n -1 << "]=" << ldl[4 * (n-1) + 3] << endl;
//	cout << "t = " << t << endl;

	double val = 1e300;
	double delta = 0 + ldl[4 * n - 1] + t;
	for (int i = 2 * n - 1; i > 0; i--) {
//		cout << "delta[" << i << "]= " << delta << endl;
		double tmp = nat[i] * ldl[2 * i] + delta;
		uru[2 * i + 1] = pivot(tmp);
		uru[2 * i    ] = nat[i] / uru[2 * i + 1];
		gamma[i] += delta ;
		delta *= ldl[2 * i - 1] / uru[2 * i + 1];
		delta += t;
		updateMinGamma(gamma[i], i, val);
	}
	uru[1]    = delta;	
	gamma[0] += delta; 

	updateMinGamma(gamma[0], 0, val);
	data[0] = mat.getShift() + t;
};
void GK::LDLToNGNPack   (const GK & mat, const double relShift){
	assert(this->representation == NGNPack && mat.getRepresentation() == LDL);
	assert(n == mat.getN() && nat == mat.getNat());
	repIndex = 2 * n - 1;
	minGammaIndex = repIndex;
	double s = 0;
	double t = relShift; // assume this will be the change in shift
	const double * RS ldl   = mat.getData();
	data[0] = ldl[0] + t;
	for (int i = 0; i < 2 * n - 1 ; i++) {
 	 	data[2 * i + 1 ] = pivot(ldl[2 * i + 1] + s + t);
		data[2 * i + 2 ] = nat[i + 1]  / data[2 * i + 1]; // L[2 * i - 1]
		s = (s + t) * data[2 * i + 2 ] * ldl[2 * i + 2];
	}
	data[4 * (n-1) + 3 ] = pivot(ldl[4 * (n-1) + 3] + s + t);
};
void GK::LDLToNative   (const GK & mat, const double shift){
	assert(this->representation == Native && mat.getRepresentation() == LDL);
	assert(n == mat.getN() && nat == mat.getNat());
	MISSING;
};
// dqds
void GK::URUToLDL      (const GK & mat, const double relShift){
	assert(this->representation == LDL    && mat.getRepresentation() == URU);
	assert(n == mat.getN() && nat == mat.getNat());
	const double t = relShift;
	const double * RS uru = mat.getData();	
	data[0] = mat.getShift() + t;
	double delta = 0 + uru[1] + t;
	for (int i = 0; i < 2 * n - 1; i++) {
		data[2 * i + 1] = pivot(nat[i+1] * uru[2 * i + 2] + delta);
		data[2 * i + 2] = nat[i+1] / data[2 * i + 1];
		delta *= uru[2 * i + 3] / data[2 * i + 1];
		delta += t;
	}
	data[4 * n - 1] = delta;
};
//	flipped dstqds
void GK::URUToURU      (const GK & mat, const double relShift){
	assert(this->representation == URU    && mat.getRepresentation() == URU);
	assert(n == mat.getN() && nat == mat.getNat());
	assert(this != &mat);

	double s = 0;
	double t = relShift; // assume this will be the change in shift
	const double * RS root = mat.getData();
	// fliping input  root[k] = root[2 * n - 1 - k]
	// fliping output data[k] = data[2 * n - 1 - k]
	// using N representation
	for (int i = 2 * n - 1; i > 0; i--) {
 	 	data[2 * i + 1 ] = pivot(root[2 * i + 1] + s + t);
		data[2 * i     ] = nat[i    ]  / data[2 * i + 1]; 
		s = (s + t) * data[2 * i    ]  * root[2 * i    ];
	}
	data[1] = pivot(root[1] + s + t);
	data[0] = root[0] + t;

};
void GK::URUToNGN      (const GK & mat, const double relShift){
	assert(this->representation == NGN    && mat.getRepresentation() == URU);
	assert(n == mat.getN() && nat == mat.getNat());
	repIndex = -1;
	// NGN to URU
	double s = 0;
	double t = relShift; // assume this will be the change in shift
	const double * RS uru     = mat.getData() ;	
	double       * RS uruPlus = data + 4 * n;	
	double       * RS gamma   = data + 8 * n;
	data[0] = uru[0] + t;
	double delta = uru[1] + t;
	for (int i = 0; i < 2 * n - 1; i++) {
		data[2 * i + 1] = pivot(nat[i+1] * uru[2 * i + 2] + delta);
		data[2 * i + 2] = nat[i+1] / data[2 * i + 1];
		delta *= uru[2 * i + 3] / data[2 * i + 1];
		delta += t;
		gamma[i] = delta;
	}
	data[4 * n - 1] = delta;

	minGammaIndex = 2 * n - 1;
	double val = fabs(gamma[2 * n - 1]);
	s = 0;
	for (int i = 2 * n - 1; i > 0; i--) {
 	 	uruPlus[2 * i + 1 ] = pivot(uru[2 * i + 1] + s + t);
		uruPlus[2 * i     ] = nat[i    ]  / uruPlus[2 * i + 1];
		gamma[i] += s ; updateMinGamma(gamma[i], i, val);
		s = (s + t) * uruPlus[2 * i    ]  * uru[2 * i    ];
	}
	uruPlus[1] = pivot(uru[1] + s + t);
	uruPlus[0] = mat.getShift() + t;
};
void GK::URUToNGNPack  (const GK & mat, const double relShift){
	assert(this->representation == NGNPack  && mat.getRepresentation() == URU);
	assert(n == mat.getN() && nat == mat.getNat());
	repIndex = 0;
	minGammaIndex = 0;
	// NGN to URU
	double s = 0;
	double t = relShift; // assume this will be the change in shift
	const double * RS uru = mat.getData() ;	
	s = 0;
	for (int i = 2 * n - 1; i > 0; i--) {
 	 	data[2 * i + 1 ] = pivot(uru[2 * i + 1] + s + t);
		data[2 * i     ] = nat[i    ]  / data[2 * i + 1];
		s = (s + t) * data[2 * i    ]  * uru[2 * i    ];
	}
	data[1] = pivot(uru[1] + s + t);
	data[0] = mat.getShift() + t;
};
void GK::URUToNative   (const GK & mat, const double shift){
	assert(this->representation == Native && mat.getRepresentation() == URU);
	assert(n == mat.getN() && nat == mat.getNat());
//	int k = mat.getRepIndex();
	MISSING;
};

void GK::NGNToLDL      (const GK & mat, const double relShift){
	assert(this->representation == LDL && mat.getRepresentation() == NGN);
	assert(n == mat.getN() && nat == mat.getNat());
	int k = mat.getRepIndex();
	double s = 0;
	double t = relShift; // assume this will be the change in shift
	const double * RS ldl = mat.getData();
	const double * RS uru = mat.getData() + 4 * n;	
	data[0] = ldl[0] + t;
	for (int i = 0; i < k ; i++) {
 	 	data[2 * i + 1 ] = pivot(ldl[2 * i + 1] + s + t);
		data[2 * i + 2 ] = nat[i + 1]  / data[2 * i + 1]; // L[2 * i - 1]
		s = (s + t) * data[2 * i + 2 ] * ldl[2 * i + 2];
	}
	if ( k == 2 * n - 1)	{
		data[4 * (n-1) + 3 ] = pivot(ldl[4 * (n-1) + 3] + s + t);
		return;
	}
	double delta = uru[2 * k + 1] - data[2 * k] * nat[k] + t;
	for (int i = k; i < 2 * n - 1; i++) {
		data[2 * i + 1] = pivot(nat[i+1] * uru[2 * i + 2] + delta);
		data[2 * i + 2] = nat[i+1] / data[2 * i + 1];
		delta *= uru[2 * i + 3] / data[2 * i + 1];
		delta += t;
	}
	data[4 * n - 1] = delta;

};
void GK::NGNToURU      (const GK & mat, const double relShift){
	assert(this->representation == URU && mat.getRepresentation() == NGN);
	assert(n == mat.getN() && nat == mat.getNat());
	int k = mat.getRepIndex();
	double s = 0;
	double t = relShift; // assume this will be the change in shift
	const double * RS ldl = mat.getData()        ;
	const double * RS uru = mat.getData() + 4 * n;
	for (int i = 2 * n - 1; i > k; i--) {
 	 	data[2 * i + 1 ] = pivot(uru[2 * i + 1] + s + t);
		data[2 * i     ] = nat[i    ]  / data[2 * i + 1]; 
		s = (s + t) * data[2 * i    ]  * uru[2 * i    ];
	}
	if ( k == 0) {
		data[1] = pivot(uru[1] + s + t);
		data[0] = mat.getShift() + t;
		return;
	}
	double delta = s + uru[2 * k + 1] + t;
	data[2 * k + 1] = pivot(delta);
	delta -= ldl[2 * k] * nat[k];
	for (int i = k; i > 0; i--) {
		data[2 * i    ] = nat[i] / data[2 * i + 1];
		delta *= ldl[2 * i - 1] / data[2 * i + 1];
		delta += t;
		data[2 * i - 1] = pivot(nat[i-1] * ldl[2 * i - 2] + delta);
	}
	data[0] = mat.getShift() + t;
};
void GK::NGNToNGN      (const GK & mat, const double relShift){
	assert(this->representation == NGN && mat.getRepresentation() == NGN);
	assert(n == mat.getN() && nat == mat.getNat());
	
	repIndex = -1;
	int k = mat.getRepIndex();
	double s = 0;
	double t = relShift; // assume this will be the change in shift
	const double * RS ldl = mat.getData();
	const double * RS uru = mat.getData() + 4 * n;	
	double * RS gamma = data + 8 * n;
	// NGN to LDL
	data[0] = ldl[0] + t;
	for (int i = 0; i < k ; i++) {
 	 	data[2 * i + 1 ] = pivot(ldl[2 * i + 1] + s + t);
		data[2 * i + 2 ] = nat[i + 1]  / data[2 * i + 1]; // L[2 * i - 1]
		gamma[i] = s;
		s = (s + t) * data[2 * i + 2 ] * ldl[2 * i + 2];
	}
	gamma[k]     = mat.getGamma()[k] + s + t;
	double delta = mat.getGamma()[k] + s + t;
	for (int i = k; i < 2 * n - 1; i++) {
		data[2 * i + 1] = pivot(nat[i+1] * uru[2 * i + 2] + delta);
		data[2 * i + 2] = nat[i+1]       / data[2 * i + 1];
		delta          *= uru[2 * i + 3] / data[2 * i + 1];
		delta += t;
		gamma[i+1] = delta;
	}
	data[4 * n - 1] = pivot(delta);
   
	minGammaIndex = 2 * n -1;
	double val = fabs(gamma[minGammaIndex]);
	
	s = 0;
	double * RS uruPlus = data + 4 * n;	
	for (int i = 2 * n - 1; i > k; i--) {
 	 	uruPlus[2 * i + 1 ] = pivot(uru[2 * i + 1] + s + t);
		uruPlus[2 * i     ] = nat[i    ]  / uruPlus[2 * i + 1];
		gamma[i] += s ; updateMinGamma(gamma[i], i, val);
		s = (s + t) * uruPlus[2 * i    ]  * uru[2 * i    ];
	}
	gamma[k] += s;
	delta = mat.getGamma()[k] + s + t;
	for (int i = k; i > 0; i--) {
 	 	uruPlus[2 * i + 1 ] = pivot(nat[i] * ldl[2 * i] + delta);
		uruPlus[2 * i     ] = nat[i    ]  / uruPlus[2 * i + 1];
		delta *= ldl[2 * i - 1] / uruPlus[2 * i + 1];
		delta += t;
		gamma[i-1] += delta; 
		updateMinGamma(gamma[i-1], i-1, val);
	}
 	uruPlus[ 1 ] = pivot(delta);
	uruPlus[ 0 ] = mat.getShift() + t;
};
void GK::NGNToNGNPack  (const GK & mat, const double relShift){
	assert(this->representation == NGNPack && mat.getRepresentation() == NGN);
	assert(n == mat.getN());
	assert(nat == mat.getNat());
	int k = repIndex = minGammaIndex = mat.getRepIndex();
	const double * RS ldl = mat.getData();
	const double * RS uru = mat.getData() + 4 * n;	
	assert( k >= 0 && k < 2 * n); // :) 
	if (relShift == 0 ) { // most of the time, I guess :) 
		for (int i = 0; i <= 2 * k; i++) 
			data[i] = ldl[i];
		data[2 * k + 1] = mat.getGamma()[k];
		for (int i = 2 * k + 2; i < 4 * n ; i++)
			data[i] = uru[i];
	} else {

		double s = 0;
		double t = relShift; // assume this will be the change in shift

		if (k > 0 && k < 2 * n - 1) {
	// NGN to LDL

		data[0] = ldl[0] + t;
		for (int i = 0; i < k ; i++) {
 	 	 	data[2 * i + 1 ] = pivot(ldl[2 * i + 1] + s + t);
			data[2 * i + 2 ] = nat[i + 1]  / data[2 * i + 1]; // L[2 * i - 1]
			s = (s + t) * data[2 * i + 2 ] * ldl[2 * i + 2];
		}
		data[2 * k + 1] =  mat.getGamma()[k] + s;
		s = 0;
		for (int i = 2 * n - 1; i > k; i--) {
 	 	 	data[2 * i + 1 ] = pivot(uru[2 * i + 1] + s + t);
			data[2 * i     ] = nat[i    ]  / data[2 * i + 1];
			s = (s + t) * data[2 * i    ]  * uru[2 * i    ];
		}
		data[2 * k + 1] += s + t;

		} else if ( k == 0 ) {
			representation = URU;
			NGNToURU(mat, relShift);	
			representation = NGNPack;
		} else if ( k == 2 * n - 1) {
			representation = LDL;
			NGNToLDL(mat, relShift);	
			representation = NGNPack;
		}
		
	}

};
void GK::NGNToNative   (const GK & mat, const double shift){MISSING};

void GK::NGNPackToLDL      (const GK & mat, const double relShift){
	assert(this->isLDL() && mat.isNGNPack());
	assert(n == mat.getN() && nat == mat.getNat());
	MISSING;
};
void GK::NGNPackToURU      (const GK & mat, const double relShift){
	assert(this->isURU() && mat.isNGNPack());
	assert(n == mat.getN() && nat == mat.getNat());
	MISSING;
};
void GK::NGNPackToNGN      (const GK & mat, const double relShift){
	assert(this->isNGN() && mat.isNGNPack());
	assert(n == mat.getN() && nat == mat.getNat());
	
	repIndex = -1;
	int k = mat.getRepIndex();

	double s = 0;
	double t = relShift; // assume this will be the change in shift
	const double * RS root = mat.getData();
	double * RS gamma = data + 8 * n;
	data[0] = root[0] + t;
	for (int i = 0; i < k ; i++) {
 	 	data[2 * i + 1 ] = pivot(root[2 * i + 1] + s + t);
		data[2 * i + 2 ] = nat[i + 1]  / data[2 * i + 1]; // L[2 * i - 1]
		gamma[i] = s;
		s = (s + t) * data[2 * i + 2 ] * root[2 * i + 2];
	}
	gamma[k]     = root[2 * k + 1] + s + t;
	double delta = root[2 * k + 1] + s + t;
	for (int i = k; i < 2 * n - 1; i++) {
		data[2 * i + 1] = pivot(nat[i+1]  * root[2 * i + 2] + delta);
		data[2 * i + 2] = nat[i+1]        / data[2 * i + 1];
		delta          *= root[2 * i + 3] / data[2 * i + 1];
		delta += t;
		gamma[i+1] = delta;
	}
	data[4 * n - 1] = pivot(delta);
   
	minGammaIndex = 2 * n -1;
	double val = fabs(gamma[minGammaIndex]);
	
	s = 0;
	double * RS uruPlus = data + 4 * n;	
	for (int i = 2 * n - 1; i > k; i--) {
 	 	uruPlus[2 * i + 1 ] = pivot(root[2 * i + 1] + s + t);
		uruPlus[2 * i     ] = nat[i    ]  / uruPlus[2 * i + 1];
		gamma[i] += s ; updateMinGamma(gamma[i], i, val);
		s = (s + t) * uruPlus[2 * i    ]  * root[2 * i    ];
	}
	gamma[k] += s;
	delta = root[2 * k + 1] + s + t;
	for (int i = k; i > 0; i--) {
 	 	uruPlus[2 * i + 1 ] = pivot(nat[i] * root[2 * i] + delta);
		uruPlus[2 * i     ] = nat[i    ]  / uruPlus[2 * i + 1];
		delta *= root[2 * i - 1] / uruPlus[2 * i + 1];
		delta += t;
		gamma[i-1] += delta; 
		updateMinGamma(gamma[i-1], i-1, val);
	}
 	uruPlus[ 1 ] = pivot(delta);
	uruPlus[ 0 ] = mat.getShift() + t;
};
void GK::NGNPackToNGNPack      (const GK & mat, const double relShift){
	assert(this->isNGNPack() && mat.isNGNPack());
	assert(n == mat.getN() && nat == mat.getNat());
	const double * RS root = mat.getData();
	int k = mat.getRepIndex();
	repIndex      = k;
	minGammaIndex = k;
	double s = 0;
	double t = relShift; // assume this will be the change in shift
	data[0] = root[0] + t;
	for (int i = 0; i < k ; i++) {
 	 	data[2 * i + 1 ] = pivot(root[2 * i + 1] + s + t);
		data[2 * i + 2 ] = nat[i + 1]  / data[2 * i + 1]; // L[2 * i - 1]
		s = (s + t) * data[2 * i + 2 ] * root[2 * i + 2];
	}
	data[2 * k + 1] = root[2 * k + 1] + s + t; // gamma_k
	s = 0;
	for (int i = 2 * n - 1; i > k; i--) {
 	 	data[2 * i + 1 ] = pivot(root[2 * i + 1] + s + t);
		data[2 * i     ] = nat[i    ]  / data[2 * i + 1];
		s = (s + t) * data[2 * i    ]  * root[2 * i    ];
	}
	data[2 * k + 1] += s;
};
void GK::NGNPackToNative      (const GK & mat, const double relShift){
	assert(this->isNative() && mat.isNGNPack());
	assert(n == mat.getN() && nat == mat.getNat());
	MISSING;
};

void GK::shiftFrom     (const GK & oth, const double relShift) {
//	cout << "shift " << relShift << " from " << names[oth.getRepresentation()];
//	cout << " to " <<  names[getRepresentation()] << endl;
//	setPivotMin(0);
	copyFields(oth);
	(this->*from[oth.getRepresentation()])(oth, relShift);
}
int GK::countLDL   (const double relShift) const {
	const double t = -relShift; // assume this will be the change in shift
	const double * RS root = getData();
	double d = root[0] + t;
	double s = 0;
	int res = 0;
	for (int i = 0; i < 2 * n - 1 ; i++) {
 	 	d = pivot(root[2 * i + 1] + s + t);
		res += d < 0.0;
		double l = nat[i + 1]  / d;
		s = (s + t) * l * root[2 * i + 2];
	}
	d = pivot(root[4 * (n-1) + 3] + s + t);
	res += d < 0.0;
	return res;
}
int GK::countURU   (const double relShift) const {
	double s = 0;
	double t = -relShift; // assume this will be the change in shift
	const double * RS root = getData();
	double r, u;
	int res = 0;
	for (int i = 2 * n - 1; i > 0; i--) {
 	 	r = pivot(root[2 * i + 1] + s + t);
		res += r < 0.0;
		u = nat[i ]  / r; 
		s = (s + t) * u  * root[2 * i    ];
	}
	r = pivot(root[1] + s + t);
	res += r < 0.0;
	return res;
}
int GK::countNGN   (const double relShift) const {

	int k = getRepIndex();
	const double t = -relShift; // assume this will be the change in shift

	const double * RS ldl = getData();
	const double * RS uru = getData() + 4 * n;	
//	double * RS     gamma = data + 8 * n;

	double s = 0;
	int res = 0;
	for (int i = 0; i < k ; i++) {
 	 	double d = pivot(ldl[2 * i + 1] + s + t);
		res += d < 0.0;
		double l = nat[i + 1]  / d;
		s = (s + t) * l * ldl[2 * i + 2];
	}
	double gamma = s;
	s = 0;
	for (int i = 2 * n - 1; i > k; i--) {
 	 	double u = pivot(uru[2 * i + 1] + s + t);
		res += u < 0.0;
		double r = nat[i    ]  / u;
		s = (s + t) * r  * uru[2 * i    ];
	}
	double delta = s + uru[2 * k + 1] + t;
	delta -= ldl[2 * k] * nat[k];
	gamma += delta ; 

	res += gamma < 0.0;


	return res;


}
int GK::countNGNPack   (const double relShift) const {
	int k = getRepIndex();
	const double t = -relShift; // assume this will be the change in shift
	double s = 0;
	int res = 0;
	for (int i = 0; i < k ; i++) {
 	 	double d = pivot(data[2 * i + 1] + s + t);
		res += d < 0.0;
		double l = nat[i + 1]  / d;
		s = (s + t) * l * data[2 * i + 2];
	}
	double gamma = data[2 *k + 1] + s;
	s = 0;
	for (int i = 2 * n - 1; i > k; i--) {
 	 	double u = pivot(data[2 * i + 1] + s + t);
		res += u < 0.0;
		double r = nat[i    ]  / u;
		s = (s + t) * r  * data[2 * i    ];
	}
	gamma += s + t ; 
	res += gamma < 0.0;
	return res;
}
int GK::countNative(const double relShift) const {
	const double shift = data[0] - relShift;
	int res = 0;
	double d = pivot(shift); 
	res += d < 0.0;
	double l;
	for (int i = 1; i < 2 * n ; i++) {
		l  = data[i] / d;
		d  = pivot(shift - data[i] * l);
		res += d < 0.0;
	}
	return res;
}
void GK::refine(Interval & inter, const double relAcc) const {
	double left, right;
	inter.getLeftRight(left, right);

	assert(relAcc >= DBL_EPSILON);
	assert(inter.count() == 1);	
	assert(callCount(left) == inter.leftCount);
	assert(callCount(right) == inter.rightCount);

	while ( inter.isTooWide(relAcc, left, right)) {
		inter.update(callCount(inter.center), left, right);
	}
}
// this method should be called to recalculated cluster boundaries, 
// therefore it should perform just a few iterations
double GK::findEigenBorder(const int wanted, const double guess, const double step) const {
	int res = callCount(guess);
	if (res == wanted) return guess;
	double left  = guess;
	double right = guess;
	double a = step; 	assert(a > 0);
	if ( res > wanted ) {
		while ((res = callCount(left - a)) > wanted ) {
			left = left - a;	a *= 2;
		}
		left = left - a;
		if ( res == wanted ) return left;
	} else {
		while ((res = callCount(right + a)) < wanted) {
			right = right + a;	a *= 2;
		}
		right  = right + a; 
		if ( res == wanted ) return right;
	}
	double center = (left + right)/2;
	while ((res = callCount(center)) != wanted) {
		left  = res < wanted ? center : left;
		right = res > wanted ? center : right;
		center = (left + right)/2;
	}
	assert(callCount(center) == wanted);
	return center;
}
// Solve T x = b
void GK::solveVecLDL    (const double * RS b, const int incB, double * RS x, const int incX)     const { 
	if (incB == 1 && incX == 1) {
		x[0] = b[0];
		for (int i = 1; i < 2 * n; i++) {
			x[i] = b[i] - data[2 * i] * x[i-1]; 
			x[i-1] /= data[2 * i - 1]; 
		}
		x[2 * n - 1] /= data[4 * n - 1]; 
		for (int i = 2 * n  - 2; i >= 0; i--) {
			x[i] -= x[i+1] * data[2 * i + 2];
		}
	} else {
		MISSING;
	}
};
void GK::solveVecURU    (const double * RS b, const int incB, double * RS x, const int incX)     const{
	if (incX == 1 && incB == 1) {
		x[2 * n - 1] = b[2 * n - 1];
		for (int i = 2 * n - 2; i >= 0; i--) {
			x[i    ]  = b[i] - data[2 * i + 2] * x[i + 1];
			x[i + 1] /= data[2 * i + 3];
		}
		x[0] /= data[1];
		for (int i = 1; i < 2 * n; i++) {
			x[i] -= data[2 * i] * x[i-1];
		}
	} else {
		MISSING;
	}
};
void GK::solveVecNative (const double * RS b, const int incB, double * RS x, const int incX)     const{MISSING};
void GK::solveVecNGN    (const double * RS b, const int incB, double * RS x, const int incX)  const{MISSING};
void GK::solveVecNGNPack    (const double * RS b, const int incB, double * RS x, const int incX)  const{MISSING};


// get Eigen 

void GK::getEigenLDL    (EIGEN_FUNC_PARAMS) const{
	if (incX == 1) {
		int k = minGammaIndex;
		k = k >= 0 ? k : 0;
		k = k < 2 * n ? k : 2 * n - 1; 
		solveCanonicalLDL(k, x, incX, xNorm2, residual, normUV); 
	} else {
		MISSING;
	}

}; 
void GK::getEigenURU    (EIGEN_FUNC_PARAMS)const{MISSING}; 
void GK::getEigenNGN    (EIGEN_FUNC_PARAMS) const {
	solveCanonicalNGN(minGammaIndex, x, incX, xNorm2, residual, normUV); 
}; 
void GK::getEigenNGNPack (EIGEN_FUNC_PARAMS) const {
	assert(minGammaIndex == repIndex);
	solveCanonicalNGNPack(minGammaIndex, x, incX, xNorm2, residual, normUV); 
}; 
void GK::getEigenNative (EIGEN_FUNC_PARAMS) const {
	solveCanonicalNative (2 * n - 1, x, incX, xNorm2, residual, normUV);
}; 

void GK::getEigenNative2 (EIGEN_FUNC_PARAMS) const {
	if (incX == 1) {
		// top to bottom solve
		x[0] = 1.0;	const	double shift = getShift(); 
		x[1] = -shift / data[1];
		int maxInd = fabs(x[1]) > fabs(x[0]); double maxVal = fabs(x[maxInd]);
		for (int i = 2; i < 2 * n; i++) {
			x[i] = -(data[i-1] * x[i-2] + shift * x[i-1]) / data[i];
			double tmp = fabs(x[i]);
			bool cond = maxVal < tmp;
			maxInd    = cond ? i         : maxInd;
			maxVal    = cond ? tmp       : maxVal; 
		}
		long double norm2 = 0; 
		minGammaIndex = maxInd;
		if (maxInd < 2 * n - 1 && maxInd > 0) {
			x[2 * n - 1] = 1.0;
			double tmp = 0.0;
			if (maxInd == 2 * n - 2) {
				tmp = x[maxInd];
			}
			x[2 * n - 2] = -shift / data[2 * n - 1];
			long double f = x[2 * n - 2] / tmp;


			norm2 = x[2 * n - 1] * x[2 * n - 1] + x[2 * n - 2] * x[2 * n - 2];

			for (int i = 2 * n - 3; i > maxInd; i--) {
				x[i] = -(x[i+2] * data[i+2] + x[i+1] * shift) / data[i+1];
				norm2 += x[i] * x[i];
			}

			int i = maxInd;
			if ( i < 2 * n - 2) {
				tmp = x[i];
				x[i] = -(x[i+2] * data[i+2] + x[i+1] * shift) / data[i+1];
				norm2 += x[i] * x[i];
				f = x[i] / tmp;
			}	


			x[i-1] *= f; norm2 += x[i-1] * x[i-1];
			residual = (x[i-1] * data[i] + x[i+1] * data[i+1] + x[i] * shift);
			residual *= x[i];

			for (int i = maxInd - 2; i >= 0;i--) {
				x[i] *= f;
				norm2 += x[i] * x[i];
			}
			xNorm2   = norm2;

		} else if (maxInd == 2 * n - 1) {
			// compute norm, residual and return
			norm2 = 0.0;
			for (int i = 0; i < 2 * n; i++) 	norm2	+= x[i] * x[i];
			residual = x[2*n-1] * (data[2*n-1] * x[2*n-2] + shift * x[2*n-1]); 
			xNorm2   = norm2;
			assert((x[2 * n - 1] * x[ 2 * n - 1] / norm2) > (0.5 / n));
		} else {
			x[2 * n - 1] = 1.0;
			x[2 * n - 2] = -shift / data[2 * n - 1];
			norm2 = x[2 * n - 1] * x[2 * n - 1] + x[2 * n - 2] * x[2 * n - 2];
			for (int i = 2 * n - 3; i >=0 ; i--) {
				x[i] = -(x[i+2] * data[i+2] + x[i+1] * shift) / data[i+1];
				norm2 += x[i] * x[i];
			}
			residual = x[0] * (x[0] * shift + data[1] * x[1]);
			xNorm2   = norm2;
		}

		assert((x[maxInd] * x[maxInd] / norm2) > (0.5 / n));
//		assert(2 * n > xNorm2); // this is equvalen of x[k] beeing above averge

	} else {
		MISSING;
	}
}; 

void GK::getEigenNative3 (EIGEN_FUNC_PARAMS) const {
	const	double shift = getShift();

	x[0] = 1.0;	 
	x[1] = -shift / data[1];
	long double topNorm = x[0] * x[0] + x[1] * x[1];
	for (int i = 2; i < 2 * n; i++) {
		x[i] = -(data[i-1] * x[i-2] + shift * x[i-1]) / data[i];
		topNorm += x[i] * x[i];
	}

	double z[2 * n];
	z[2 * n - 1] = 1.0;
	z[2 * n - 2] = -shift / data[2 * n - 1];
	long double botNorm = z[2 * n - 1] * z[2 * n - 1] + 
								 z[2 * n - 2] * z[2 * n - 2];
	for (int i = 2 * n - 3; i >= 0; i--) {
		z[i] = -(z[i+2] * data[i+2] + z[i+1] * shift) / data[i+1];
		botNorm += z[i] * z[i];

	}

	// find K-index that is good enough for computing the vector
	double topLimit = topNorm * 0.5 / n;
	double botLimit = botNorm * 0.5 / n;
	int found = -1; double bestVal = 0;
	for (int i = 0; i < 2 * n; i++) {
		if ( x[i] * x[i] > topLimit && z[i] * z[i] > botLimit) {
			double tmp = x[i] * x[i] * z[i] * z[i];
			found = tmp > bestVal ? i : found; 
			bestVal = fmax(tmp, bestVal);
		}
	}
	cout << "x = ";
	for (int i = 0; i < 2 * n; i++)
		cout << x[i] << " "; cout << endl;
	cout << "z = ";
	for (int i = 0; i < 2 * n; i++)
		cout << z[i] << " "; cout << endl;
	cout << found << endl;	


}
void GK::solveCanonicalLDL   (const int k, EIGEN_FUNC_PARAMS) const {
	if (incX == 1) {
		assert( k>= 0 && k < 2 * n);
		x[0] = k == 0 ? 1.0 : 0.0;

		for (int i = 1; i < k; i++) {
			x[i  ]  = - data[2 * i] * x[i-1]; 
			x[i-1] /=   data[2 * i - 1]; 
		}

		if ( k > 0) {
			x[k] = 1.0 - data[2 * k] * x[k-1]; 
			x[k-1] /= data[2 * k - 1]; 
		}

		for (int i = k+1; i < 2 * n; i++) {
			x[i  ]  = -data[2 * i] * x[i-1]; 
			x[i-1] /=  data[2 * i - 1]; 
		}

		x[2 * n - 1] /= data[4 * n - 1]; 

		for (int i = 2 * n  - 2; i >= 0; i--) {
			x[i] -= x[i+1] * data[2 * i + 2];
		}
	} else {
		MISSING;
	}
}
void GK::solveCanonicalURU   (const int k, EIGEN_FUNC_PARAMS) const {
	MISSING;
}
void GK::solveCanonicalNGN   (const int k, EIGEN_FUNC_PARAMS) const {
	if (incX == 1) {
		assert( k>= 0 && k < 2 * n);
		x[k] = 1.0;
		const double * RS ldl = getData();
		const double * RS uru = getData() + 4 * n;
		normUV[0] = 0; normUV[1] = 0;
		for (int i = k - 1; i >= 0; i--) {
			x[i] = - ldl[2 * i + 2] * x[i+1];
			normUV[i & 1] += x[i] * x[i];
		}
		for (int i = k + 1; i < 2 * n; i++) {
			x[i] = - uru[2 * i] * x[i-1];
			normUV[i & 1] += x[i] * x[i];
		}
		normUV[k & 1] += 1.0;
		xNorm2   = normUV[0] + normUV[1];
		residual = getGamma()[k]; 	
//		assert(2 * n > xNorm2); // this is equvalen of x[k] beeing above averge
	} else {
		MISSING;
	}
}
void GK::solveCanonicalNGNPack   (const int k, EIGEN_FUNC_PARAMS) const {
	if (incX == 1) {
		assert( k>= 0 && k < 2 * n);
		x[k] = 1.0;
		normUV[0] = 0; normUV[1] = 0;
		for (int i = k - 1; i >= 0; i--) {
			x[i] = - data[2 * i + 2] * x[i+1];
			normUV[i & 1] += x[i] * x[i];
		}
		residual = data [ 2 * k + 1]; 	
		for (int i = k + 1; i < 2 * n; i++) {
			x[i] = - data[2 * i] * x[i-1];
			normUV[i & 1] += x[i] * x[i];
		}
		normUV[k & 1] += 1.0;
		xNorm2   = normUV[0] + normUV[1];
	} else {
		MISSING;
	}
}
void GK::solveCanonicalNGN2  (const int k, EIGEN_FUNC_PARAMS) const {
	if (incX == 1) {
		assert( k>= 0 && k < 2 * n);
		x[k] = 1.0; 
		const	double shift = getShift();
		// following code is simplified assuming that x[k] = 1.0
		long double norm1 = 0;
		if ( k > 0 ) {
			assert(x[k] == 1.0);
			x[k-1] = -getL(k-1);
			norm1 += x[k-1] * x[k-1];
			for (int i = k - 2; i >= 0; i--) {
				// data[i + 2] = i & 1 == 0 ? e[(i / 2)] : d[(i/2) + 1 ]; 
				// data[i + 1] = i & 1 == 0 ? d[(i / 2)] : e[(i/2) + 1 ]; 
				x[i] = -(x[i+2] * getNative(i+2) + x[i+1] * shift) / getNative(i+1);
				norm1 += x[i] * x[i];
			}
		}
		long double norm2 = 0;
		if ( k < 2 * n - 1) {
			assert(x[k] == 1.0);
			x[k+1] = -getU(k);
			norm2 += x[k+1] * x[k+1];
			for (int i = k + 2; i < 2 * n; i++) {
				x[i] = -(getNative(i-1) * x[i-2] + shift * x[i-1]) / getNative(i);
				norm2 += x[i] * x[i];
			}
		}
		assert(x[k] == 1.0);
		xNorm2   = norm1 + 1.0 + norm2;
		residual = getGamma()[k]; 	
//		assert(2 * n > xNorm2); // this is equvalen of x[k] beeing above averge
	} else {
		MISSING;
	}
}
void GK::solveCanonicalNative(const int k, EIGEN_FUNC_PARAMS) const {
	if (incX == 1) {
		if (k == 2 * n - 1) {
			x[0] = 1.0;
			double shift = getShift(); 
			x[1] = -shift / data[1];
			long double norm2 = x[0] * x[0] + x[1] * x[1];
			for (int i = 2; i < 2 * n; i++) {
				x[i] = -(data[i-1] * x[i-2] + shift * x[i-1]) / data[i];
				norm2 += x[i] * x[i];
			}
			xNorm2    = norm2;
			residual  = data[2 * n - 1] * x[2 * n - 2] + shift * x[2 * n - 1]; 
			residual *= data[2 * n - 1];
		}
	} else {
		MISSING;
	}
}
double GK::checkRepresentationLDL   (int & k) const {
	double shift = getShift();
	k = 2 * n - 1; // LDL is equvalend to NGN with index 2 * n - 1
	double res = fabs(shift - data[1]); 
	double tmp;
	for (int i = 1; i < n ; i++) {
		// diagonal = d_i + lld_i-1
		tmp = data[4 * i - 1] +  data[4 * i - 2] * nat[2*i -1];//d[i-1];
		res = fmax(res, fabs(tmp - shift));
		tmp = data[4 * i + 1] +  data[4 * i    ] * nat[2*i   ];//e[i-1];
		res = fmax(res, fabs(tmp - shift));
	}
	tmp = data[4 * n - 1] +  data[4 * n - 2] * nat[2*n -1];//d[n-1];
	res = fmax(res, fabs(tmp-shift));
	return res;
}
double GK::checkRepresentationURU   (int & k) const {
	double shift = getShift();
	double res = fabs(shift - data[4 * n - 1]); 
	double tmp = data[4 * n  - 3 ] + data[4 * n  - 2] * nat[2*n-1];//d[n-1];
	res = fmax(res, fabs(tmp-shift));
	for (int i = n - 1; i > 0 ; i--) {
		tmp = data[4 * i - 1 ] + data[4 * i + 0] * nat[2*i];// e[i-1];
		res = fmax(res, fabs(tmp - shift));
		tmp = data[4 * i - 3 ] + data[4 * i - 2] * nat[2*i -1];//d[i-1]; 
		res = fmax(res, fabs(tmp - shift));
	}
	k = 0;
	return res;
}
double GK::checkRepresentationNGN   (int & k) const {
	double * RS ldl   = data;
	double * RS uru   = data + 4 * n;
	double * RS gamma = uru  + 4 * n;
	assert(n >= 2);
	double shift = getShift();
	k = 2 * n - 1 ; // LDL is equvalend to NGN with index 2 * n - 1
	double resLDL[2 * n]; 
	double elgLDL[2 * n];
	resLDL[0] = fabs(shift - ldl[1]); 
	elgLDL[0] = fabs(ldl[1]);
	double tmp;
	for (int i = 1; i < 2 * n ; i++) {
		tmp = ldl[2 * i + 1] +  ldl[2 * i] * nat[i    ];//d[i-1];
		resLDL[i] = fmax(resLDL[i-1], fabs(tmp - shift));
		elgLDL[i] = fmax(fabs(ldl[2 * i + 1]), fabs(ldl[2 * i + 2]));
		elgLDL[i] = fmax(elgLDL[i], elgLDL[i - 1]);
	}


	double resURU[2 * n];
	double elgURU[2 * n]; 
	resURU[2 * n - 1] = fabs(shift - uru[4 * n - 1]); 
	elgURU[2 * n - 1] = fabs(uru[4 * n - 1]);
	for (int i = 2 * n - 1; i > 0 ; i--) {
		tmp = uru[2 * i - 1 ] + uru[2 * i + 0] * nat[i];
		resURU[i - 1] = fmax(resURU[i], fabs(tmp - shift));
		elgURU[i-1] = fmax(fabs(uru[2 * i - 1]), fabs(uru[2 * i]));
		elgURU[i-1] = fmax(elgURU[i-1], elgURU[i]);
	}

	double resNGN[2 * n]; 
	double elgNGN[2 * n]; 
	for (int i = 0; i < 2 * n; i++) resNGN[i] = -1;
	resNGN[0] = resURU[0];
	elgNGN[0] = elgURU[0];
	double val = getShiftness(resNGN[0], elgNGN[0]);
	double tieBraker = fabs(gamma[0]);
	k = 0;
	for (int i = 1; i < 2 * n - 1 ; i++) {
		tmp = ldl[2 * i    ] * nat[i  ] +  
				uru[2 * i + 2] * nat[i+1] + 
				gamma[i] ;
		double absGamma = fabs(gamma[i]);
		resNGN[i] = fmax(resLDL[i - 1], resURU[i + 1]);
		resNGN[i] = fmax(fabs(tmp - shift), resNGN[i    ]);
		elgNGN[i] = fmax(elgURU[i + 1], elgLDL[i-1]);
		elgNGN[i] = fmax(elgNGN[i], absGamma);
		
		tmp = getShiftness(resNGN[i], elgNGN[i]);
		if ( tmp <= val  ) {
			if ( tmp < val || absGamma < tieBraker) {
				val = tmp;
				tieBraker = absGamma;	
				k  = i;
			}
		}
	}
	int i = 2 * n - 1;
	elgNGN[i] = elgLDL[i-1];
	resNGN[i] = resLDL[i-1];
	double absGamma = fabs(gamma[i]);
	tmp = getShiftness(resNGN[i], elgNGN[i]);
	if ( tmp <= val  ) {
		if ( tmp < val || absGamma < tieBraker) {
			val = tmp;
			tieBraker = absGamma;	
			k  = i;
		}
	}
	return val;
	
}
void GK::checkRepresentationAllNGN(double * error) const {
	assert(isNGN());
	const double * RS ldl   = data;
	const double * RS uru   = data + 4 * n;
	const double * RS gamma = data + 8 * n;
	const double totShift   = data[0];

	for (int k = 0; k < 2 * n; k++) {
		double lld = 0;
		double max = 0;
		for (int i = 0; i < k; i++) {
			double tmp = ldl[2 * i + 1] + lld - totShift;
			max = fmax(fabs(tmp), max);		
			lld = nat[i+1] * ldl[ 2 * i + 2];
		}
		double uur = 0;
		for (int i = 2 * n - 1; i > k; i--) {
			double tmp = uru[2 * i + 1] + uur - totShift;
			max = fmax(fabs(tmp), max);		
			uur = nat[i] * uru[2 * i ];
		}
		double tmp = gamma[k] + lld + uur - totShift;
		max = fmax(fabs(tmp), max);		
		error[k] = max;
	}
}
double GK::checkRepresentationNGNPack   (int & k) const {

	double lld = 0;
	double max = 0;
	double elg = 0;
	const double totShift   = data[0];
	k = repIndex;
	for (int i = 0; i < k; i++) {
		double tmp = data[2 * i + 1] + lld - totShift;
		max = fmax(fabs(tmp), max);		
		lld = nat[i+1] * data[ 2 * i + 2];

		elg = fmax(elg, fabs(data[2 * i + 1]));
		elg = fmax(elg, fabs(data[2 * i + 2]));
	}
	double uur = 0;
	for (int i = 2 * n - 1; i > k; i--) {
		double tmp = data[2 * i + 1] + uur - totShift;
		max = fmax(fabs(tmp), max);		
		uur = nat[i] * data[2 * i ];

		elg = fmax(elg, fabs(data[2 * i + 1]));
		elg = fmax(elg, fabs(data[2 * i    ]));
	}
	elg = fmax(elg, fabs(data[2 * k + 1]));
	double tmp = data[2 * k + 1] + lld + uur - totShift;
	max = fmax(fabs(tmp), max);		

	return  getShiftness(max, elg);
}
double GK::checkRepresentationNative(int & k) const {
	return 0;
}
void GK::setPointers() {
	switch (representation) {
		case Native : {
			from[Native] = &GK::nativeToNative;
			from[LDL   ] = &GK::LDLToNative;
			from[URU   ] = &GK::URUToNative;
			from[NGN   ] = &GK::NGNToNative;
			initialize = &GK::initializeNative;
			solve      = &GK::solveVecNative;
			getVec     = &GK::getEigenNative3;
			print      = &GK::printNative;
			addShift   = &GK::addShiftNative;
			check      = &GK::checkRepresentationNative;
			count		  = &GK::countNative;
			break;
		};
		case LDL : {
			from[Native  ] = &GK::nativeToLDL   ;
			from[LDL     ] = &GK::LDLToLDL   ;
			from[URU     ] = &GK::URUToLDL   ;
			from[NGN     ] = &GK::NGNToLDL   ;
			from[NGNPack ] = &GK::NGNPackToLDL;
			initialize = &GK::initializeLDL;
			solve      = &GK::solveVecLDL;
			getVec     = &GK::getEigenLDL;
			print      = &GK::printLDL;
			addShift   = &GK::addShiftLDL;
			check      = &GK::checkRepresentationLDL;
			count		  = &GK::countLDL;
			break;
		};
		case URU : {
			from[Native  ] = &GK::nativeToURU   ;
			from[LDL     ] = &GK::LDLToURU   ;
			from[URU     ] = &GK::URUToURU   ;
			from[NGN     ] = &GK::NGNToURU   ;
			from[NGNPack ] = &GK::NGNPackToURU;

			initialize = &GK::initializeURU;
			solve      = &GK::solveVecURU;
			getVec     = &GK::getEigenURU;
			print      = &GK::printURU;
			addShift   = &GK::addShiftURU;
			check      = &GK::checkRepresentationURU;
			count		  = &GK::countURU;
			break;
		}
		case NGN : {
			from[Native  ] = &GK::nativeToNGN   ;
			from[LDL     ] = &GK::LDLToNGN   ;
			from[URU     ] = &GK::URUToNGN   ;
			from[NGN     ] = &GK::NGNToNGN   ;
			from[NGNPack ] = &GK::NGNPackToNGN;
			initialize = &GK::initializeNGN;
			solve      = &GK::solveVecNGN;
			getVec     = &GK::getEigenNGN;
			print      = &GK::printNGN;
			addShift   = &GK::addShiftNGN;
			check      = &GK::checkRepresentationNGN;
			count		  = &GK::countNGN;
			break;
		}
		case NGNPack : {
	 		from[Native      ] = &GK::nativeToNGNPack;
			from[LDL         ] = &GK::LDLToNGNPack;
			from[URU         ] = &GK::URUToNGNPack;
			from[NGN         ] = &GK::NGNToNGNPack;
			from[NGNPack     ] = &GK::NGNPackToNGNPack;
			initialize   = &GK::initializeNGNPack;
			solve        = &GK::solveVecNGNPack;
			getVec       = &GK::getEigenNGNPack;

			print        = &GK::printNGNPack;
			addShift     = &GK::addShiftNGNPack;
			check        = &GK::checkRepresentationNGNPack;
			count		    = &GK::countNGNPack;

			break;
		};
		default: { MISSING };
	}
}
int GK::chooseRepIndex(const double * RS s2, double & val) { //envelope vector
// given an envelope vector, 
// we choose the rep index in such way,
// that 
// min_k |G_k * s|^2
	if (isNGN()) {
		double sumDS[2 * n];
		const double * RS gamma = getGamma();
		const double * RS uru   = getData() + 4 * n;
		sumDS[0] = 0;
		const double * RS ldl = getData();	
		for (int i = 1; i < 2 * n; i++) {
			double d2 = ldl[2 * i - 1] * ldl[2 * i - 1];
			sumDS[i] = s2[i-1] * d2;
		}
		int bestTwistIndex = 2 * n - 1;
		double sumRS = 0;
		double g2  = gamma[2 * n - 1] * gamma[2 * n - 1];
		val = sumDS[2 * n - 1] + g2 * s2[ 2 * n -1];
		for (int i = 2 * n - 2; i >= 0; i--) {
			sumRS += s2[i] * uru[2 * i + 3];
			g2 = gamma[i] * gamma[i];
			double tmp = sumRS + g2 * s2[i] + sumDS[i];
			bool cond  = tmp < val;
			val = cond ? tmp : val;
			bestTwistIndex = cond ? i : bestTwistIndex;
		}
		repIndex = bestTwistIndex;
		return bestTwistIndex;
	} else {
		switch (representation) {
			case LDL : { return 2 * n - 1; };
			case URU : { return 0        ; };
			case NGNPack : { return repIndex;};
			case NGN : { assert(false); return -1;};
			case Native: { assert(false); return -1; };
		}
	}
	return -1;	
}
std::ostream& operator<<(std::ostream& os, const GK & m) {
	const char * factNames [] = {"Native", "LDL", "URU", "NGN", "NGNPack"};
	int n = m.getN();
	GK::Representation rep = m.getRepresentation();
	os << endl << "GK Matrix " << 2 * n << "-by-" << 2 * n;
	os << " factorization = " << factNames[rep] << endl;
	os.precision(16);
	os << "accumulated shift = " << m.getShift() << '\n';
	os << "spdiam = "    << m.spdiam;
	os << " maxNat = "   << m.maxNat;
	os << " pivotMin = " << m.pivotMin;
	os << " repIndex = " << m.repIndex << '\n'; 
	os << " signature = " << m.getSig() << '\n';
	int k;
	double repErr = m.checkRepresentation(k);
	os << "rep. error " << repErr << " index = " << k << endl;

	m.callPrint(os);
	return os;	
}

void GK::printLDL   (std::ostream & os) const{
	os << "D = ";
	for (int i = 0; i < 2 * n; i++) os << data[2 * i + 1] << " ";
	os << "\nL = ";
	for (int i = 0; i < 2 * n - 1; i++) os << data[2 * i + 2] << " ";
	os << endl;
}
void GK::printURU   (std::ostream & os) const{
	os << "R = ";
	for (int i = 0; i < 2 * n; i++) os << data[2 * i + 1] << " ";
	os << "\nU = ";
	for (int i = 0; i < 2 * n - 1; i++) os << data[2 * i + 2] << " ";
	os << endl;
}
void GK::printNGN   (std::ostream & os) const{
	os << "D = ";
	for (int i = 0; i < 2 * n; i++)     os << data[2 * i + 1] << " ";
	os << "\nL = ";
	for (int i = 0; i < 2 * n - 1; i++) os << data[2 * i + 2] << " ";
	os << "\nR = ";
	for (int i = 0; i < 2 * n; i++)     os << data[4 * n + 2 * i + 1] << " ";
	os << "\nU = ";
	for (int i = 0; i < 2 * n - 1; i++) os << data[4 * n + 2 * i + 2] << " ";
	os << "\nG = ";
	for (int i = 0; i < 2 * n; i++)     os << data[8 * n + i    ] << " ";
	assert(minGammaIndex >= 0 && minGammaIndex < 2 * n);
	os << "\nG[" << minGammaIndex << "] = " << getGamma()[minGammaIndex] << endl;
	double err[2 * n];
	checkRepresentationAllNGN(err);
	os << "\n abs rep err = ";
	for (int i = 0; i < 2 * n; i++) {
		os << err[i] << " ";
	}
	os << "\n";

}
void GK::printNGNPack   (std::ostream & os) const{
	int k = getRepIndex();
	os << "D = ";
	for (int i = 0; i < k ;  i++) os << data[2 * i + 1] << " ";
	os << "\nL = ";
	for (int i = 0; i < k ; i++)  os << data[2 * i + 2] << " ";
	os << "\nGamma = " << data[2 * k + 1];
	os << "\nR = ";
	for (int i = k + 1; i < 2 * n; i++) os << data[2 * i + 1] << " ";
	os << "\nU = ";
	for (int i = k + 1; i < 2 * n; i++) os << data[2 * i    ] << " ";
	os << "\n";

}
void GK::getResidual (const double * RS vec, const int incVec, 
					double * RS res      , const int incRes) const {
	assert(nat != 0);
	if (incVec == 1 && incRes == 1) {
		const double shift = getShift();
		res[0] = shift * vec[0] + nat[1] * vec[1]; 
		for (int i = 0; i < n-1; i++) {
			int k = 2 * i;
			res[k+1] = nat[k+1] * vec[k+0] + shift*vec[k+1] + nat[k+2]*vec[k+2];
			res[k+2] = nat[k+2] * vec[k+1] + shift*vec[k+2] + nat[k+3]*vec[k+3];
		}
		res[2 * n - 1] = shift * vec[2 * n - 1] + nat[2*n-1] * vec[2 * n - 2]; 
	} else {
		MISSING;
	}
}
void GK::printNative(std::ostream & os) const{
	os << "OffDiagonal = ";
	for (int i = 1; i < 2 * n; i++)     os << data[i] << " ";
	os << endl;
}
GKBisector::GKBisector(const GK & gk){
	n = gk.getN();
	uru = new double[2 * n];
	selfAllocated = true;
	initialize(gk);	
};
GKBisector::GKBisector(const GK & gk, double * RS storage){
	n = gk.getN();
	uru = storage;
	selfAllocated = false;
	initialize(gk);	
}
void GKBisector::initialize(const GK & gk) {
	all = NULL; 
	const double * RS nat = gk.getNat();
	max = 0;
	pivotMin = 0;
	for (int i = 0; i < 2 * n - 2; i++) {
		max = fmax(nat[i+1] + nat[i+2], max);
		uru[i] = nat[i+1] * nat[i+1];
		pivotMin = fmax(uru[i], pivotMin);
	}
	uru[2 * n - 2] = nat[2 * n -1 ] * nat[2 * n - 1];
	uru[2 * n - 1] = 0; 
	pivotMin = fmax(uru[2 * n  - 1], pivotMin);
	relativeWidthLimit = DBL_EPSILON;
	pivotMin *= DBL_EPSILON;
	gapTol = 1e-3;
}

GKBisector::~GKBisector() {
	if ( uru != NULL && selfAllocated) {
		delete [] uru;
	}
	uru = NULL;
}
// returns the number of singular values, less then lambda
// complexity should be O(n)
int GKBisector::count(const double t) const {
	int count = 0;
	
	const long double t2 = - t * t;
	long double s = 0;
	for (int i = n - 1; i > 0; i--) {
		long double rplus = pivot(uru[2 * i    ] + s + t2);
		count += rplus < 0;
		long double uplus = uru[2 * i - 1 ] / rplus;
		s = (s + t2) * uplus;
	}
	long double rplus = pivot(uru[0] + s + t2);	
	count += rplus < 0;
	return count;
}

void GKBisector::identifyClusters(Interval inter[] ) const {
/* After calling this function you can interate over all 
 * clusters / singletons with the following example code
 
	i = 0;
	int startCluster = 0;
	int stopCluster = 0;

	while (i < n) {
		int j = inter[i].rightCount;
		if (j == n || !inter[i].isClustered(gapTol, inter[j])) {
			stopCluster = inter[i].rightCount - 1;
			if (stopCluster == startCluster) { // singleton found
				// add in queue singelton task
				cout << "Singleton found " << i << endl;
//				cout << inter[i];
				

				// refine it to full accuracy
				refine(inter[i]);
				
			} else { // cluster found
				// add in queue cluster task
				cout << "Cluster found " << startCluster << " " << 
					stopCluster << endl;
			}
			startCluster = j;
		}
		i = j;
	}
*/
	inter[0].width      = (max );
	inter[0].center     = (max)/2;
	inter[0].leftCount  = 0;
	inter[0].rightCount = n;
	
	assert( relAcc  < gapTol / 4);
	double left  = 0  ;
	double right = max;
	
	assert(gapTol >= 1e-5);
	int i = 0;
	while ( i < n ) {
		inter[i].getLeftRight(left, right);
		while ( inter[i].isTooWide(relAcc) ) {
			int countCenter = count(inter[i].center);
			if ( countCenter == inter[i].leftCount || 
				  countCenter == inter[i].rightCount ) { 
				inter[i].update(countCenter, left, right);
			} else {
				// split the interval
				inter[i].split(countCenter, left, right, inter[countCenter]);
				if (inter[i].count() == 1) { break;	}; 
			}
		}
		
		if ( inter[i].count() == 1) 	
			while ( inter[i].isTooWide(relAcc) &&
					  inter[i].width > DBL_EPSILON) {
				int countCenter = count(inter[i].center);
				inter[i].update(countCenter, left, right);
			}

		i = inter[i].rightCount; // go to next interval
	}
}
void GKBisector::refine(Interval & inter) const {

	double left, right;	inter.getLeftRight(left, right);

	assert(inter.count() == 1);
	assert(relativeWidthLimit >= DBL_EPSILON);
	while (inter.isTooWide(relativeWidthLimit, left, right)) {
		int countCenter = count(inter.center);

		if (!( (countCenter == inter.leftCount) xor 
				 (countCenter == inter.rightCount) ) ) {
			cout << inter << " count in center " << countCenter << endl;
		}

		assert((countCenter == inter.leftCount) xor 
				 (countCenter == inter.rightCount) );

		left  = countCenter == inter.leftCount  ? inter.center :  left;
		right = countCenter == inter.rightCount ? inter.center : right;
		inter.center = (left + right)/2;
		inter.width  = right - left;
		inter.getLeftRight(left, right);
	}

}
void GKBisector::refine(const int i) const {
	assert(all != NULL && i >= 0 && i < n);
	double left, right;
	Interval & inter = all[i];
	inter.getLeftRight(left, right);
	while ( inter.isTooWide(relAcc, left, right)) {
		inter.update(count(inter.center), left, right);
	}
}
int GKBisector::split(const int i) const {
	double left, right;
	all[i].getLeftRight(left, right);
	while ( all[i].isTooWide(relAcc) ) {
		int countCenter = count(all[i].center);
		if ( countCenter == all[i].leftCount || 
			  countCenter == all[i].rightCount ) { 
			all[i].update(countCenter, left, right);
		} else {
			// split the allval
			all[i].split(countCenter, left, right, all[countCenter]);
			return countCenter;
		}
	}
	return i;
}
#include "UnitTester.h"
UNITTEST(TestGK);
TestGK::TestGK() {
	setName 				("Test GK");
	setDescription  	("Test GK for correct bahaviour & memory allocation");
	setTestAuthor		("Yordan Madzhunkov");
	setDeveloperAuthor("Yordan Madzhunkov");
	setCompileTime(__DATE__, __TIME__); // each class that inherits this one should also have this line.
};
bool TestGK::test() const {
 	double ar1[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
	17, 18, 19, 20, 21, 22, 23, 24, 25 };
	const int n = 10;
	Matrix Bi (n, n, BiDiagonal, ar1);

	GK gk1Nat(Bi, 0);
	GK gk1(NGN, gk1Nat, 1.23);
	gk1.chooseRepIndex();

	double z[2 * n]; double normUV[2]; 
	double norm2, residual, correction = 1.0;

	// Perform max 10 Rayleigh quotient iteration to refine the eigenvalue
	gk1.getEigen(z, 1, norm2, residual, normUV);
	correction = -residual/norm2;
	int maxIter = 10, iter = 1;
	while ( fabs(correction) >  gk1.getShift() * 1e-15 && iter++ < maxIter) {
		gk1 += correction;
		gk1.getEigen(z, 1, norm2, residual, normUV);
		correction = -residual/norm2;
	}
	cout.precision(16);
	bool pass = (gk1.getShift() - 9.226029434232607e-05) < 1e-19;
	pass &= 2 * n > norm2;
	assert(pass);



	GK gk2(NGN, gk1Nat, 1e-5);
	gk1.shiftFrom(gk1Nat, 15 -0.4088233546525065 + 0.01092462882854788 + 3.383157238346357e-05 );
//	cout << gk1 << endl;
		gk1.setMinGammaIndex(5);
		gk1.getEigen(z, 1, norm2, residual, normUV);
		correction = -residual/norm2;
//		cout << "5" << " " << correction <<  " " << norm2 << endl;
	gk2.shiftFrom(gk1, correction);
//	cout << gk2 << endl;

	maxIter = 10, iter = 1;
//	while ( fabs(correction) >  gk1.getShift() * 1e-15 && iter++ < maxIter) {
		gk2.shiftFrom(gk1, correction);
		gk2.getEigen(z, 1, norm2, residual, normUV);
		correction = -residual/norm2;
//	cout << gk2 << endl;
//		if (fabs(correction) <=  gk2.getShift() * 1e-15 && iter++ < maxIter)
//			break;
//		gk1.shiftFrom(gk1, correction);
//		gk1.getEigen(z, 1, norm2, residual);
//		correction = -residual/norm2;
//	}

//	cout << "AFTER" << endl;	
//	cout << "norm2 " << norm2 << endl;





	gk2.shiftFrom(gk1Nat, 0.23);
	gk2.getEigen(z, 1, norm2, residual, normUV);

	correction = -residual/norm2;
	
	maxIter = 10; iter = 1;
	while ( fabs(correction) >  gk2.getShift() * 1e-15 && iter++ < maxIter) {
		gk2 += correction;
		gk2.setMinGammaIndex(2 * n - 1);
		gk2.getEigen(z, 1, norm2, residual, normUV);
		correction = -residual/norm2;
	}
	pass &= 2 * n > norm2;
	assert(pass);
	pass &= (gk2.getShift() - 9.226029434232607e-05) < 1e-19;
	assert(pass);

	double res[2 * n];
		
	gk1.getResidual(z, 1, res, 1);
//	for (int i = 0; i < 2 * n; i++) cout << z[i] << " ";
//	cout << endl;
//	for (int i = 0; i < 2 * n; i++) cout << res[i] << " ";
//	cout << endl;
//	cout << gk1 << endl;
	double u[n], v[n];
	const double rn = sqrt(2.0 / norm2);
	const double mrn = -rn;
	long double u2 = 0, v2 = 0;
	for (int i = 0; i < n; i++) {
		u[i] = z[2 * i + 1] * mrn; u2 += u[i] * u[i];
		v[i] = z[2 * i    ] * rn ; v2 += v[i] * v[i];
	}
//	cout << u2 - 1 << " " << v2 - 1 << endl;
//	pass &= fabs(u2 - 1) < 2 * DBL_EPSILON;
//	pass &= fabs(v2 - 1) < 2 * DBL_EPSILON;
	assert(pass);

	double dHard [] = {1, 2, 3, -4, 5, 6, 7, 8, 9, 10, 
		1.0/8, 2.0/8, 3.0/8,	4.0/8, 5.0/8, 6.0/8, 7.0/8, 8.0/8, 9.0/8};
	Matrix B(n, n, BiDiagonal, dHard); assert(B.isUpper());
	GK BGK1(B, 0.0);
	GK BGK (NGN, BGK1, 0.997383);
	
	BGK.getEigen(z, 1, norm2, residual, normUV);
	correction = -residual/norm2;
	maxIter = 10; iter = 1;
	
	while ( fabs(correction) >  BGK.getShift() * 1e-15 && iter++ < maxIter) {
		BGK += correction;
		BGK.getEigen(z, 1, norm2, residual,normUV);
		correction = -residual/norm2;
	}
//	cout.precision(18);
	pass &= 2 * n > norm2;

	// check (LDL)z = b && URUz = b
	assert(pass);
	
	for (int r = 1; r < 3; r++) {	  
		GK B1( static_cast<GK::Representation>(r), BGK1, 1.2345);
		B1.setPivotMin(DBL_EPSILON * 4);
		double b[2 * n]; 
		for (int i = 0; i < 2 * n; i++) b[i] = 1.0; 	
		B1.solveVec   (b, 1, z, 1);
	 	B1.getResidual(z, 1, b, 1);
		norm2 = 0;
		for (int i = 0; i < 2 * n; i++) {
			b[i] -= 1.0;
			norm2 += (b[i] * b[i]) ;
		}
		pass &=  norm2 / (2 * n * DBL_EPSILON * DBL_EPSILON)< 5.0;
	
	}
// test shifting	
	assert(pass);
	GK nat(B, 0);

	double shift1 = 2.0;
	double shift2 = -shift1 + 0.24;

	int k = -1;

	bool printTable = false;
	int w = 15;
	const char * labels[] = {
		"LDL", "URU", "NGN_0", "NGN_19", "NGN_10", "NGN_auto"
	};	
	if (printTable) {
		cout << endl;
		cout.precision(5);
		cout << setw(w) << " " << "   ";
		cout << setw(w) << " ";
		for (int i = 1; i <= 6; i++) 
				cout << setw(w) << labels[i-1]  << " ";
		cout << endl;
	}


	for (int r = 1; r <= 6; r++) {
		if (printTable)
			cout << setw(w) << labels[r-1]  << " ";
		GK::Representation rep = r <= 3 ? 
			static_cast<GK::Representation>(r) : NGN;
		GK child1( rep, nat, shift1);
		if ( r >= 3) {
			if (r == 3) child1.setRepIndex(0);
			if (r == 4) child1.setRepIndex(2* n - 1);
			if (r == 5) child1.setRepIndex(10);
		}
		if (printTable)
			cout << setw(w) << child1.checkRepresentation(k)  << " ->";
		for (int i = 1; i <= 6; i++) {
			GK::Representation rep = i <= 3 ? 
				static_cast<GK::Representation>(i) : NGN;
			GK child2( rep, child1, shift2);
			if (printTable)
				cout << setw(w) << child2.checkRepresentation(k)  << " ";
		}
		if (printTable)
			cout << endl;
		
	}
	if (printTable) {
		cout << setw(w) << " " << " ";
		cout << setw(w) << nat.checkRepresentation(k)  << " ->";
	}
	for (int i = 1; i <= 6; i++) {
		GK::Representation rep = i <= 3 ? 
			static_cast<GK::Representation>(i) : NGN;
		GK child2( rep, nat, shift1 + shift2);
		if (printTable) 
			cout <<  std::setw(w) << child2.checkRepresentation(k)  << " ";
	}
	if (printTable) 
		cout << endl;

	assert(pass);
	GK ldl( LDL, nat, 2.0);
	GK uru( URU, nat, 2.0);
	GK ngn( NGN, nat, 2.0);
	double limit1 = 1e-13;	
//	cout << '\n';
//	cout << "Native to LDL " << ldl.checkRepresentation(k) << endl;
	pass &=  ldl.checkRepresentation(k) < limit1;
//	cout << "Native to URU " << uru.checkRepresentation(k) << endl;
	pass &=  uru.checkRepresentation(k) < limit1;
//	cout << "Native to NGN " << ngn.checkRepresentation(k) << endl;
	pass &=  ngn.checkRepresentation(k) < limit1;
	assert(pass);


	GK ldl2(LDL, ldl, 0.5);
//	cout << "LDL to LDL " << ldl2.checkRepresentation(k) << endl;
	pass &=  ldl2.checkRepresentation(k) < limit1;
	assert(pass);
   GK uru2(URU, uru, 0.01);

 //	cout << "URU to URU " << uru2.checkRepresentation(k) << endl;
	pass &=  uru2.checkRepresentation(k) < limit1;

   GK ldl3(LDL, uru, 0.5);
//	cout << "URU -> LDL" <<  ldl3 << endl;

//	cout << 	ldl3.checkRepresentation(k);
//	cout << " " << k << endl;
	pass &=  ldl3.checkRepresentation(k) < limit1;
	GK uru3(URU, ldl, 0.5);
	assert(pass);

//	cout << "LDL -> URU" <<  uru3 << endl;
//	cout << uru3.checkRepresentation(k);
//	cout << " " << k << endl;
	
	pass &=  uru3.checkRepresentation(k) < limit1;

   GK ngn3(NGN, ldl, 0.5);

//	cout << ngn3 << endl;
	pass &=  ngn3.checkRepresentation(k) < limit1;
	assert(pass);

	GK ldl4(LDL, ngn, 0.5);

//	cout << "LDL -> NGN " << ldl4.checkRepresentation(k) << endl;
	pass &= ldl4.checkRepresentation(k)  < limit1;

	GK uru4(URU, ngn, 0.5);

//	cout << "LDL -> NGN " << uru4.checkRepresentation(k) << endl;
	pass &= uru4.checkRepresentation(k)  < limit1;
	ngn.setRepIndex(0);
	GK ngn4(NGN, ngn, 0.5);
	pass &= ngn4.checkRepresentation(k)  < limit1;
	
//	cout << ngn4 << endl;
	ngn.setRepIndex(10);
	
	GK pack1(NGNPack, ngn, 0);
	pass &= pack1.checkRepresentation(k)  < limit1;
	GK pack2(NGNPack, pack1, 0.5);
	pass &= pack2.checkRepresentation(k)  < limit1;
//	cout << pack2 << endl;
	GK ngn6(NGN, pack1, 0.5);
	pass &= ngn6.checkRepresentation(k)  < limit1;
//	cout << ngn6 << endl;
	GK pack3 (NGNPack, ngn, 0.5);
	pass &= pack3.checkRepresentation(k)  < limit1;
//	cout << pack3 << endl;
//	test add shift :) 
	pack1 += 0.5;
	pass &= pack1.checkRepresentation(k)  < limit1;


	// test bisection and cluster classification
	assert(pass);

	GKBisector bis( nat ) ;
	// nat has the followinf singular values
	// 10.2891299546928323 9.00724709978768523 7.97107425087430155
	// 6.97271747345694592 5.97661256325546031 4.98057100904744665
	// 3.98454752282449043 2.98856032475197919 1.99267289366755262
	// 0.997383294572933843 
//	cout << bis.count(1) << endl;
	pass &= bis.count(1) == 1;
	
	pass &= bis.count(3.9845479   ) == 4;
	pass &= bis.count(3.9845472282) == 3;

	pass &= bis.count(6.97271747345694592        ) == 6 ;
	pass &= bis.count(6.97271747345694592 + 6e-16) == 7;

	Interval interval;
	interval.center = 7.0;
	interval.width  = 0.5;
	interval.leftCount  = 6;	
	interval.rightCount = 7;	
	bis.refine(interval);
//	cout << interval.center << " +/- " << interval.width/2 << endl;


	Interval all[n]; memset(all, 0, sizeof(all));

	bis.setGapTol(1e-3);
	bis.setRelAcc(1e-8);

	bis.identifyClusters(all);
/*  
	for (int i = 0; i < n; i++) {
		cout << all[i].center << " +/- " << all[i].width/2 ;
		cout << " count = " << all[i].count() << endl;
	}
	
*/

//	pass &=  uru2.checkRepresentation(k) < 5e-15;

//	cout <<"u= ";for (int i = 0; i < n; i++) cout << u[i] << " "; 
//	cout <<  "norm2 = " << u2 << endl;
//	cout <<"v= ";for (int i = 0; i < n; i++) cout << v[i] << " ";
//	cout <<  "norm2 = " << v2 << endl;
	
//	cout << "z= ";
//	for (int i = 0; i < 2 * n; i++) 
//		cout << z[i] << " "; cout << endl;

	return pass;
}

TestGK testGK;

